/**************************************************
*
*			FTC Tool Kit
*			Used for FTC teams 5954 and 7032
*
*/


// Drivers Used In-Line
#include "drivers/HTSPB-driver.h"
//#include "IRDriver/hitechnic-irseeker-v2.h"

// Data Definitions
#define DEADZONE 10
#define MIN_MOTOR_POWER 20
#define PICKUP_POWER 100
#define BALL_DROP_POS 225
#define BALL_HOLD_POS 255
#define IR_SERVO_UP 255
#define IR_SERVO_DOWN 160
#define TOW_POWER 50
#define GRAB_GOAL_POWER 100
#define CONVEYOR_POWER 35

// Limit Switches
#define lHIGHEXTEND_HIGH 	0b00000001
#define lHIGHEXTEND_LOW 	0b00000010
#define lLOWEXTEND_HIGH 	0b00000100
#define lLOWEXTEND_LOW 		0b00001000
//#define lLOWEXTEND_MID 		0b00010000
#define lUP_CONVEYOR 			0b00100000
#define lDOWN_CONVEYOR 		0b01000000

// Auton Enc Values 5954

#define DEG_45_TURN_5954 	550
#define DEG_90_TURN_5954 	1300
#define DEG_180_TURN_5954 2400

// Auton Enc Value 7032
#define DEG_45_TURN_7032 	650
#define DEG_90_TURN_7032 	1275
#define DEG_180_TURN_7032 2400

#define LEFT false
#define RIGHT true

int DEG_45_TURN, DEG_90_TURN, DEG_180_TURN;

// Needed Prototypes
bool ReadLimit(unsigned char mSensor);

/******************************************
*
*							Control Standards
*
*******************************************/

string TeamName = "1";
void ImplementVariables(void){
	getFriendlyName(TeamName);
	if (TeamName == "5954")
	{
		DEG_45_TURN = DEG_45_TURN_5954;
		DEG_90_TURN = DEG_90_TURN_5954;
		DEG_180_TURN = DEG_180_TURN_5954;
	}
	else
	{
		DEG_45_TURN = DEG_45_TURN_7032;
		DEG_90_TURN = DEG_90_TURN_7032;
		DEG_180_TURN = DEG_180_TURN_7032;
}
}

int DeadZone(int mVal){
	return ( abs(mVal) > DEADZONE ? mVal : 0 );
}

void waitForTask(bool *memLoc){
	while(*memLoc){
		abortTimeslice();
	}
}

void waitForLimit(unsigned char mSensor, bool mCondition){
	while(ReadLimit(mSensor) != mCondition){
		abortTimeslice();
	}
}

void waitForMTime(int time){
	ClearTimer(T3);
	while(time1[T3] < time){
		abortTimeslice();
	}
}



/******************************************
*
*							Sensor Code
*
*******************************************/

char ReadIRBeacon(void){
	servo[sIRSeeker] = IR_SERVO_UP;
	waitForMTime(1000);

	char irBeacon = SensorValue[tIRBeacon];

	servo[sIRSeeker] = IR_SERVO_DOWN;
	waitForMTime(100);

	return irBeacon;
}

bool ReadLimit(unsigned char mSensor){
	return ( HTSPBreadIO(tLimits, mSensor) > 0 ? true : false );
}

/******************************************
*
*							Drive Code
*
*******************************************/

void DriveSystem(int mPower, int mDirection){
	mPower = mPower * -1;

	motor[dLeft1] = motor[dLeft2] = DeadZone(mPower - mDirection);
	motor[dRight1] = motor[dRight2] = DeadZone(mPower + mDirection);
}

void TeleopDriveSystem(int mPower, int mDirection){
	DriveSystem(mPower * 100 / 127, mDirection * 100 / 127);
}

/******************************************
*
*							Arm Control
*
*******************************************/

void LowExtension(int mPower){
	nxtDisplayTextLine(1, "1: %d  2: %d", ReadLimit(lLOWEXTEND_LOW),ReadLimit(lLOWEXTEND_HIGH));
	motor[aLExtend] = ( ( mPower <  0 && ReadLimit(lLOWEXTEND_LOW)) || ( mPower >  0 && ReadLimit(lLOWEXTEND_HIGH)) ? 0 : DeadZone(mPower));
}

void HighExtension(int mPower){
	motor[aUExtend] = ( ( mPower <  0 && ReadLimit(lHIGHEXTEND_LOW)) || ( mPower >  0 && ReadLimit(lHIGHEXTEND_HIGH)) ? 0 : DeadZone(mPower));
}

void ScoreBalls (bool mActivate){
	servo[sBallDrop] = mActivate * BALL_DROP_POS + !mActivate * BALL_HOLD_POS;
}

/******************************************
*
*							Ball Control
*
*******************************************/

void BallPickup(bool mIn, bool mOut){
	motor[aBallPickup] = mIn * PICKUP_POWER - mOut * PICKUP_POWER;
}

void BallDrop(bool mActivate){
	servo[sBallDrop] = mActivate * BALL_DROP_POS + !mActivate * BALL_HOLD_POS;
}

bool bToggle2 = true;
void BallPickupTip(bool mUp, bool mDown){
	if (mUp){
		bToggle2 = true;
	}
	else if (mDown){
		bToggle2 = false;
	}
	motor[aBallTip] = !bToggle2 * !ReadLimit(lDOWN_CONVEYOR) * CONVEYOR_POWER - bToggle2 * !ReadLimit(lUP_CONVEYOR) * CONVEYOR_POWER;
}

/******************************************
*
*							Tow System
*
*******************************************/

void TowTeleop(bool mUp, bool mDown){
	motor[mTow] = mUp * TOW_POWER + mDown * TOW_POWER * -1;
}

/******************************************
*
*							Autonimous Mode Functions
*
*******************************************/
int Test_Enc = 0;
void EncoderDrive(int mPower, int mDirection, int mPosition, bool mReset){
	if (mReset) nMotorEncoder[dLeft1] = 0;
	wait1Msec(40);
	DriveSystem(mPower, mDirection);
	while(!(abs(nMotorEncoder[dLeft1]) > (mPosition - 30))){
		abortTimeslice();
	}
	DriveSystem(0, 0);
	wait1Msec(100);
}

//void PIDDrive(int mPower, int mDirection, int mPosition, bool mReset){
//	if (mReset) nMotorEncoder[dLeft1] = 0;
//	nMotorEncoderTarget[dLeft1] = mPosition;
//	wait1Msec(40);
//	DriveSystem(mPower, mDirection);
//	while(nMotorRunState[dLeft1] != runStateIdle){
//		abortTimeslice();
//	}
//	DriveSystem(0, 0);
//	wait1Msec(20);
//}

/******************************************
*
*							TASKS
*
*******************************************/

bool tReleaseBoom_Active = false;
task ReleaseBoom(){
	tReleaseBoom_Active = true;

	LowExtension(100);
	waitForLimit(lLOWEXTEND_HIGH, true);

	LowExtension(-100);
	waitForLimit(lLOWEXTEND_LOW, true);

	LowExtension(0);
	wait1Msec(20);
	tReleaseBoom_Active = false;
}

bool tDropTow_Active = false;
task DropTow(){
	tDropTow_Active = true;
	TowTeleop(false, true);
	ClearTimer(T2);
	while(time1[T2] < 1800){
		abortTimeslice();
	}
  TowTeleop(false, false);
  tDropTow_Active = false;
}

bool tRaiseTow_Active = false;
task RaiseTow(){
	tRaiseTow_Active = true;

	TowTeleop(true, false);
	ClearTimer(T2);
	while(time1[T2] < 2400){
		abortTimeslice();
	}
  TowTeleop(false, false);
  tRaiseTow_Active = false;
}

bool tHighExtendUp_Active = false;
task HighExtendUp(){
	tHighExtendUp_Active = true;

	HighExtension(100);
	waitForLimit(lHIGHEXTEND_HIGH, true);
	HighExtension(0);

	tHighExtendUp_Active = false;
}

bool tLowExtendUp_Active = false;
task LowExtendUp(){
	tLowExtendUp_Active = true;

	LowExtension(100);
	waitForLimit(lLOWEXTEND_HIGH, true);
	LowExtension(0);

	tLowExtendUp_Active = false;
}

bool tLowExtendDown_Active = false;
task LowExtendDown(){
	tLowExtendDown_Active = true;

	LowExtension(-100);
	waitForLimit(lLOWEXTEND_LOW, true);
	LowExtension(0);

	tLowExtendDown_Active = false;
}

bool tHighExtendDown_Active = false;
task HighExtendDown(){
	tHighExtendDown_Active = true;

	HighExtension(-100);
	waitForLimit(lHIGHEXTEND_LOW, true);
	HighExtension(0);

	tHighExtendDown_Active = false;
}

bool tReloadBall_Active = false;
task ReloadBall(){
	tReloadBall_Active = true;

	BallPickup(true, false);
	ClearTimer(T1);
	while(time1[T1] < 2000){
		abortTimeslice();
	}
	BallPickup(false, false);

	tReloadBall_Active = false;
}

bool tPositionHighGoal_Active = false;
task PositionHighGoal(){
	tPositionHighGoal_Active = true;
	StartTask(HighExtendUp);
	StartTask(LowExtendUp);

	waitForTask(&tLowExtendUp_Active);
	waitForTask(&tHighExtendUp_Active);
	tPositionHighGoal_Active = false;
}

bool tScoreBall_Active = false;
task ScoreBall(){
	tScoreBall_Active = true;

	StartTask(HighExtendUp);
	waitForTask(&tHighExtendUp_Active);


	ClearTimer(T1);
	while(time1[T1] < 2000){
		abortTimeslice();
	}
	servoChangeRate[sBallDrop] = 1;
	ScoreBalls(true);
	ClearTimer(T1);
	while(time1[T1] < 2000){
		abortTimeslice();
	}
	ScoreBalls(false);
	servoChangeRate[sBallDrop] = 10;
	tScoreBall_Active = false;
}

/*
bool tScanIR_Active = false;
task ScanIR(){
	tScanIR_Active = true;
	servo[sIRSeeker] = IR_SERVO_UP;


}*/
